#include "rads_map.hpp"

int main(int argc, char** argv)
{
    ros::init(argc, argv, NODE_MAP);
    ros::NodeHandle nh;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(MAP_PCD_TEST, *cloud) == -1)
    {
        ROS_ERROR("Couldn't read the pcd file");
        return (-1);
    }
    ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("/pradsmap", QUEUE_SIZE);
    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        sensor_msgs::PointCloud2 ros_cloud;
        pcl::toROSMsg(*cloud, ros_cloud);
        ros_cloud.header.stamp = ros::Time::now();
        ros_cloud.header.frame_id = "nyd";
        pub.publish(ros_cloud);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}